#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
30档速度标定脚本 (支持线速度和角速度)
30-Level Speed Calibration Script (Support Linear and Angular Speed)

该脚本用于标定六足机器人的30档速度，支持线速度和角速度的独立标定
This script calibrates 30 speed levels for hexapod robot with separate linear and angular speed calibration

作者: YAHBOOM | Gentle Xu
Author: YAHBOOM | Gentle Xu
"""

import time
import json
import os
import numpy as np
import threading
from typing import Dict, List, Tuple, Optional
from datetime import datetime

try:
    from muto_hexapod_lib.MutoLargemodelInterface import MutoController
except ImportError as e:
    print(f"❌ 导入MutoLargemodelInterface错误: {e}")
    print("请确保已正确安装muto_hexapod_lib包")
    exit(1)

class SpeedCalibration30Levels:
    """
    30档速度标定器 (支持线速度和角速度)
    30-Level Speed Calibrator (Support Linear and Angular Speed)
    """
    
    def __init__(self, port: str = "/dev/myserial", debug: bool = True, calibration_type: str = "linear"):
        """
        初始化标定器
        Initialize calibrator
        
        Args:
            port: 串口端口 Serial port
            debug: 调试模式 Debug mode
            calibration_type: 标定类型 "linear" 或 "angular" Calibration type "linear" or "angular"
        """
        self.debug = debug
        self.robot = None
        self.calibration_data = {}  # 存储标定数据
        self.interpolated_data = {}  # 存储插值数据
        self.calibration_type = calibration_type  # 标定类型
        
        # 标定配置
        if calibration_type == "linear":
            self.calibration_levels = [1, 5, 10, 15, 20, 25, 30]  # 线速度标定档位
            self.calibration_distance_m = 0.5  # 线速度标定距离(米)
        else:  # angular
            self.calibration_levels = [10, 15, 20]  # 角速度标定档位(限制范围10-20)
            self.calibration_angle_deg = 360  # 角速度标定角度(度)
        self.calibration_repeats = 1  # 每个档位重复测试次数(改为1次)
        
        # 撤回功能支持
        self.calibration_history = []  # 标定历史记录，支持撤回
        self.input_history = []  # 输入历史，用于撤回功能
        
        # 初始化机器人
        try:
            self.robot = MutoController(port=port, debug=debug)
            print("✅ 机器人初始化成功")
        except Exception as e:
            print(f"❌ 机器人初始化失败: {e}")
            raise
    
    def calibrate_single_level(self, level: int, test_param: float = None) -> Dict:
        """
        交互式标定单个档位（支持线速度和角速度标定）
        Interactive calibration of single speed level (support linear and angular speed)
        
        Args:
            level: 速度档位 Speed level (1-30)
            test_param: 测试参数 - 线速度时为距离(m)，角速度时为角度(deg)
            
        Returns:
            Dict: 标定结果 Calibration result
        """
        if test_param is None:
            if self.calibration_type == "linear":
                test_param = self.calibration_distance_m
            else:  # angular
                test_param = self.calibration_angle_deg
        
        calibration_name = "线速度" if self.calibration_type == "linear" else "角速度"
        unit = "m/s" if self.calibration_type == "linear" else "deg/s"
        
        print(f"\n🔧 标定{calibration_name}档位 {level}")
        print("选择标定方式:")
        print(f"1. 机器人{('移动' if self.calibration_type == 'linear' else '旋转')}测试（按回车）")
        print(f"2. 直接输入{calibration_name}值（输入 'd'）")
        print("3. 撤回上一次输入（输入 'u'）")
        
        while True:
            choice = input("请选择 [回车/d/u]: ").strip().lower()
            
            if choice == 'u':
                # 撤回功能
                if self.input_history:
                    last_input = self.input_history.pop()
                    if last_input['level'] in self.calibration_data:
                        del self.calibration_data[last_input['level']]
                    print(f"✅ 已撤回档位 {last_input['level']} 的标定结果")
                    return None  # 返回None表示撤回操作
                else:
                    print("⚠️  没有可撤回的操作")
                    continue
            
            elif choice == 'd':
                # 直接输入速度值
                while True:
                    try:
                        speed_input = input(f"请直接输入档位 {level} 的{calibration_name}值（{unit}）: ")
                        speed_value = float(speed_input)
                        if speed_value < 0:
                            print(f"{calibration_name}不能为负数，请重新输入")
                            continue
                        
                        # 创建直接输入的结果
                        if self.calibration_type == "linear":
                            calibration_result = {
                                'level': level,
                                'avg_time_s': 3.0,
                                'avg_speed_ms': speed_value,
                                'std_speed_ms': 0.0,
                                'avg_distance_m': speed_value * 3.0,
                                'repeats': 1,
                                'raw_results': [{'speed_ms': speed_value, 'time_s': 3.0, 'distance_m': speed_value * 3.0}],
                                'timestamp': datetime.now().isoformat(),
                                'input_method': 'direct'
                            }
                        else:  # angular
                            calibration_result = {
                                'level': level,
                                'avg_time_s': 3.0,
                                'avg_angular_speed_deg_s': speed_value,
                                'std_angular_speed_deg_s': 0.0,
                                'avg_angle_deg': speed_value * 3.0,
                                'repeats': 1,
                                'raw_results': [{'angular_speed_deg_s': speed_value, 'time_s': 3.0, 'angle_deg': speed_value * 3.0}],
                                'timestamp': datetime.now().isoformat(),
                                'input_method': 'direct'
                            }
                        
                        # 记录到输入历史
                        self.input_history.append({
                            'level': level,
                            'result': calibration_result,
                            'method': 'direct'
                        })
                        
                        print(f"✅ 档位 {level} 直接输入完成: {speed_value:.3f} {unit}")
                        return calibration_result
                        
                    except ValueError:
                        print("请输入有效的数字")
            
            elif choice == '' or choice == '\n':
                # 机器人移动测试
                break
            else:
                print("无效选择，请重新输入")
        
        # 执行机器人测试
        if self.calibration_type == "linear":
            print(f"机器人将以档位 {level} 的速度前进 3 秒")
        else:  # angular
            print(f"机器人将以档位 {level} 的角速度持续旋转，您需要观察旋转360度后按回车停止")
        
        if self.calibration_type == "linear":
            input("按回车键开始测试...")
        
        print("准备开始测试...")
        time.sleep(1)  # 给用户准备时间
        
        # 记录开始时间
        start_time = time.time()
        
        # 执行测试
        try:
            if self.calibration_type == "linear":
                # 线速度测试：固定时间移动
                move_duration = 3.0  # 固定移动3秒
                end_time = start_time + move_duration
                
                print("开始移动！")
                while time.time() < end_time:
                    if self.robot:
                        self.robot.move(level, 0, 0)  # 前进
                    time.sleep(0.05)  # 20Hz控制频率
                
                # 停止机器人
                if self.robot:
                    self.robot.move(0, 0, 0)
                print("移动完成！")
                
                actual_time = time.time() - start_time
                
                # 询问用户实际移动距离
                while True:
                    try:
                        distance_input = input(f"请输入机器人实际移动的距离（米）[输入'u'撤回]: ")
                        
                        if distance_input.strip().lower() == 'u':
                            print("取消本次测试")
                            return None
                        
                        actual_distance = float(distance_input)
                        if actual_distance < 0:
                            print("距离不能为负数，请重新输入")
                            continue
                        break
                    except ValueError:
                        print("❌ 输入的数值无效！")
                        print("请选择以下操作:")
                        print("1. 进入下一个档位的标定 (默认，直接按回车)")
                        print("2. 重新输入距离值 (输入 'r')")
                        print("3. 重新标定当前档位 (输入 'c')")
                        
                        error_choice = input("请选择 [回车/r/c]: ").strip().lower()
                        
                        if error_choice == 'r':
                            continue  # 重新输入距离
                        elif error_choice == 'c':
                            print("🔄 重新标定当前档位...")
                            return self.calibrate_single_level(level, test_param)  # 递归重新标定
                        else:  # 默认选择：进入下一个档位
                            print("⏭️ 跳过当前档位，进入下一个档位的标定")
                            return None  # 返回None表示跳过当前档位
                
                measured_speed = actual_distance / actual_time if actual_time > 0 else 0
                
            else:  # angular 角速度测试
                # 角速度测试：用户控制开始和停止
                print("机器人将开始持续旋转，请等待旋转360度后按回车停止")
                input("按回车键开始旋转...")
                
                print("开始旋转！机器人正在旋转，请等待旋转360度后按回车停止...")
                start_time = time.time()  # 重新记录开始时间
                
                # 开始持续旋转，使用循环保持旋转状态
                rotation_active = True
                
                def rotation_loop():
                    while rotation_active and self.robot:
                        self.robot.move(0, 0, level)  # 持续发送旋转指令
                        time.sleep(0.05)  # 20Hz控制频率
                
                # 启动旋转线程
                if self.robot:
                    rotation_thread = threading.Thread(target=rotation_loop)
                    rotation_thread.daemon = True
                    rotation_thread.start()
                
                # 等待用户按回车停止
                input()  # 用户按回车停止旋转
                
                # 停止旋转
                rotation_active = False
                if self.robot:
                    self.robot.move(0, 0, 0)
                    time.sleep(0.1)  # 确保停止指令生效
                
                end_time = time.time()
                actual_time = end_time - start_time
                
                print(f"旋转停止！旋转时间: {actual_time:.2f}秒")
                
                # 固定角度为360度
                actual_angle = 360.0
                measured_angular_speed = actual_angle / actual_time if actual_time > 0 else 0
            
            # 创建标定结果
            if self.calibration_type == "linear":
                calibration_result = {
                    'level': level,
                    'avg_time_s': actual_time,
                    'avg_speed_ms': measured_speed,
                    'std_speed_ms': 0.0,  # 单次测试无标准差
                    'avg_distance_m': actual_distance,
                    'repeats': 1,
                    'raw_results': [{
                        'time_s': actual_time,
                        'speed_ms': measured_speed,
                        'distance_m': actual_distance
                    }],
                    'timestamp': datetime.now().isoformat(),
                    'input_method': 'test'  # 标记为测试获得
                }
                
                print(f"测试结果:")
                print(f"  移动时间: {actual_time:.2f}秒")
                print(f"  移动距离: {actual_distance:.3f}米")
                print(f"  实际线速度: {measured_speed:.3f}米/秒")
                
            else:  # angular
                calibration_result = {
                    'level': level,
                    'avg_time_s': actual_time,
                    'avg_angular_speed_deg_s': measured_angular_speed,
                    'std_angular_speed_deg_s': 0.0,  # 单次测试无标准差
                    'avg_angle_deg': actual_angle,
                    'repeats': 1,
                    'raw_results': [{
                        'time_s': actual_time,
                        'angular_speed_deg_s': measured_angular_speed,
                        'angle_deg': actual_angle
                    }],
                    'timestamp': datetime.now().isoformat(),
                    'input_method': 'test'  # 标记为测试获得
                }
                
                print(f"测试结果:")
                print(f"  旋转时间: {actual_time:.2f}秒")
                print(f"  旋转角度: {actual_angle:.1f}度")
                print(f"  实际角速度: {measured_angular_speed:.1f}度/秒")
            
            # 记录到输入历史
            self.input_history.append({
                'level': level,
                'result': calibration_result,
                'method': 'test'
            })
            
            return calibration_result
            
        except Exception as e:
            print(f"❌ 测试过程出错: {e}")
            return None
    
    def calibrate_fixed_levels(self) -> Dict:
        """
        交互式标定固定间隔的档位
        Interactive calibration of fixed interval levels
        
        Returns:
            Dict: 标定结果 Calibration results
        """
        print("\n🚀 开始交互式档位标定")
        print(f"将要标定的档位: {self.calibration_levels}")
        print(f"每个档位重复测试: {self.calibration_repeats} 次")
        print("\n注意事项:")
        print("1. 每次测试前请确保机器人处于安全位置")
        print("2. 准备好测量工具（如卷尺）")
        print("3. 机器人将移动3秒，请观察并测量实际移动距离")
        
        input("\n按回车键开始标定流程...")
        
        calibration_results = {}
        
        for i, level in enumerate(self.calibration_levels):
            print(f"\n{'='*50}")
            print(f"标定进度: {i+1}/{len(self.calibration_levels)} - 档位 {level}")
            print(f"{'='*50}")
            
            result = self.calibrate_single_level(level)
            if result:
                calibration_results[level] = result
                # 根据标定类型使用不同的字段
                if self.calibration_type == "linear":
                    self.calibration_data[level] = result['avg_speed_ms']
                else:  # angular
                    self.calibration_data[level] = result['avg_angular_speed_deg_s']
                print(f"✅ 档位 {level} 标定成功")
            else:
                print(f"❌ 档位 {level} 标定失败")
                retry = input("是否重新标定此档位？(y/n): ")
                if retry.lower() == 'y':
                    result = self.calibrate_single_level(level)
                    if result:
                        calibration_results[level] = result
                        # 根据标定类型使用不同的字段
                        if self.calibration_type == "linear":
                            self.calibration_data[level] = result['avg_speed_ms']
                        else:  # angular
                            self.calibration_data[level] = result['avg_angular_speed_deg_s']
                        print(f"✅ 档位 {level} 重新标定成功")
            
            # 询问是否继续
            if i < len(self.calibration_levels) - 1:
                continue_calibration = input(f"\n是否继续标定下一个档位 {self.calibration_levels[i+1]}？(y/n): ")
                if continue_calibration.lower() != 'y':
                    print("标定流程已中断")
                    break
        
        print(f"\n✅ 标定流程完成，成功标定 {len(calibration_results)} 个档位")
        
        return calibration_results
    
    def interpolate_missing_levels(self) -> Dict:
        """
        基于运动控制物理特性的智能插值计算
        Intelligent interpolation based on movement control physics
        
        在move方法中，档位参数直接作为运动强度控制参数传递给底层系统
        档位越高，运动幅度越大，但速度与档位的关系可能是非线性的
        插值时需要考虑运动控制系统的实际特性
        
        Returns:
            Dict: 插值结果 Interpolation results
        """
        print("\n🔄 开始基于运动控制特性的智能插值...")
        
        if len(self.calibration_data) < 2:
            print("❌ 标定数据不足，无法进行插值")
            return {}
        
        # 准备插值数据
        levels = sorted(self.calibration_data.keys())
        speeds = [self.calibration_data[level] for level in levels]
        
        print(f"基准数据点: {dict(zip(levels, speeds))}")
        
        # 分析运动控制特性
        print("\n📈 分析运动控制特性...")
        
        # 计算档位间的速度变化率
        speed_gradients = []
        for i in range(1, len(levels)):
            level_diff = levels[i] - levels[i-1]
            speed_diff = speeds[i] - speeds[i-1]
            gradient = speed_diff / level_diff if level_diff > 0 else 0
            speed_gradients.append(gradient)
            print(f"  档位 {levels[i-1]}-{levels[i]}: 速度变化率 {gradient:.4f} m/s per level")
        
        # 分析速度变化模式
        if len(speed_gradients) >= 2:
            avg_gradient = np.mean(speed_gradients)
            gradient_std = np.std(speed_gradients)
            print(f"\n平均速度变化率: {avg_gradient:.4f} ± {gradient_std:.4f} m/s per level")
            
            # 检测是否存在非线性特征
            gradient_variation = gradient_std / avg_gradient if avg_gradient > 0 else 0
            if gradient_variation > 0.3:
                print(f"⚠️  检测到显著非线性特征 (变异系数: {gradient_variation:.2f})")
                use_nonlinear = True
            else:
                print(f"✅ 运动特性相对线性 (变异系数: {gradient_variation:.2f})")
                use_nonlinear = False
        else:
            avg_gradient = speed_gradients[0] if speed_gradients else 0.01
            use_nonlinear = False
            print(f"\n使用单一梯度: {avg_gradient:.4f} m/s per level")
        
        # 智能插值策略 - 仅针对10-20范围内的档位
        interpolated_results = {}
        
        # 角速度标定只处理10-20范围内的档位
        if self.calibration_type == "angular":
            target_range = range(10, 21)  # 10-20档位
        else:
            target_range = range(1, 31)  # 线速度保持原有范围
        
        for target_level in target_range:
            if target_level not in self.calibration_data:
                
                if use_nonlinear:
                    # 非线性插值：使用分段线性插值
                    interpolated_speed = np.interp(target_level, levels, speeds)
                    
                    # 应用局部梯度修正
                    # 找到最近的标定点
                    closest_level_idx = min(range(len(levels)), 
                                           key=lambda i: abs(levels[i] - target_level))
                    
                    if closest_level_idx < len(speed_gradients):
                        local_gradient = speed_gradients[closest_level_idx]
                    elif closest_level_idx > 0:
                        local_gradient = speed_gradients[closest_level_idx - 1]
                    else:
                        local_gradient = avg_gradient
                    
                    # 基于局部梯度的微调
                    closest_level = levels[closest_level_idx]
                    level_offset = target_level - closest_level
                    gradient_correction = level_offset * (local_gradient - avg_gradient) * 0.3
                    
                    final_speed = interpolated_speed + gradient_correction
                    
                else:
                    # 线性插值：基于平均梯度
                    # 找到插值区间
                    lower_level = None
                    upper_level = None
                    
                    for level in levels:
                        if level < target_level:
                            lower_level = level
                        elif level > target_level and upper_level is None:
                            upper_level = level
                            break
                    
                    if lower_level is not None and upper_level is not None:
                        # 区间内线性插值
                        lower_speed = self.calibration_data[lower_level]
                        upper_speed = self.calibration_data[upper_level]
                        level_ratio = (target_level - lower_level) / (upper_level - lower_level)
                        final_speed = lower_speed + (upper_speed - lower_speed) * level_ratio
                    elif lower_level is not None:
                        # 向上外推
                        lower_speed = self.calibration_data[lower_level]
                        final_speed = lower_speed + (target_level - lower_level) * avg_gradient
                    elif upper_level is not None:
                        # 向下外推
                        upper_speed = self.calibration_data[upper_level]
                        final_speed = upper_speed - (upper_level - target_level) * avg_gradient
                    else:
                        # 默认估算
                        final_speed = target_level * avg_gradient
                
                # 确保速度为正值且合理
                final_speed = max(0.001, final_speed)
                
                interpolated_results[target_level] = final_speed
                self.interpolated_data[target_level] = final_speed
                
                # 详细日志（仅显示部分）
                if target_level <= 5 or target_level % 5 == 0:
                    method = "非线性" if use_nonlinear else "线性"
                    print(f"  档位 {target_level:2d}: {method}插值 -> {final_speed:.3f} m/s")
        
        print(f"\n✅ 智能插值完成，计算了 {len(interpolated_results)} 个档位")
        
        # 显示插值结果验证
        if interpolated_results:
            print("\n📊 插值结果验证:")
            
            # 检查速度递增的合理性
            all_levels = sorted(list(self.calibration_data.keys()) + list(interpolated_results.keys()))
            all_speeds = [self.calibration_data.get(level, interpolated_results.get(level, 0)) for level in all_levels]
            
            # 计算实际的速度变化率
            actual_gradients = []
            for i in range(1, len(all_levels)):
                level_diff = all_levels[i] - all_levels[i-1]
                speed_diff = all_speeds[i] - all_speeds[i-1]
                gradient = speed_diff / level_diff if level_diff > 0 else 0
                actual_gradients.append(gradient)
            
            if actual_gradients:
                avg_actual_gradient = np.mean(actual_gradients)
                gradient_std = np.std(actual_gradients)
                print(f"  插值后平均速度变化率: {avg_actual_gradient:.4f} ± {gradient_std:.4f} m/s per level")
                print(f"  速度变化一致性: {(1-gradient_std/avg_actual_gradient)*100:.1f}%" if avg_actual_gradient > 0 else "N/A")
                
                # 显示关键档位的插值结果
                print("\n关键档位插值结果:")
                key_levels = [5, 10, 15, 20, 25, 30]
                for level in key_levels:
                    if level in interpolated_results:
                        speed = interpolated_results[level]
                        print(f"  档位 {level:2d}: {speed:.3f} m/s")
        
        return interpolated_results
    
    def generate_complete_calibration_table(self) -> Dict:
        """
        生成完整的标定表
        角速度标定：仅生成10-20档位的标定表
        线速度标定：生成完整的30档标定表
        Generate complete calibration table
        Angular calibration: only generate 10-20 level calibration table
        Linear calibration: generate complete 30-level calibration table
        
        Returns:
            Dict: 完整标定表 Complete calibration table
        """
        complete_table = {}
        
        # 根据标定类型确定范围
        if self.calibration_type == "angular":
            level_range = range(10, 21)  # 角速度只处理10-20档位
        else:
            level_range = range(1, 31)  # 线速度处理1-30档位
        
        # 合并标定数据和插值数据
        for level in level_range:
            if level in self.calibration_data:
                speed_value = self.calibration_data[level]
                complete_table[level] = {
                    'speed_ms' if self.calibration_type == "linear" else 'angular_speed_deg_s': speed_value,
                    'source': 'calibrated'
                }
            elif level in self.interpolated_data:
                speed_value = self.interpolated_data[level]
                complete_table[level] = {
                    'speed_ms' if self.calibration_type == "linear" else 'angular_speed_deg_s': speed_value,
                    'source': 'interpolated'
                }
            else:
                # 使用线性估算
                if self.calibration_type == "angular":
                    estimated_speed = level * 2.0  # 角速度估算：每档位约2度/秒
                else:
                    estimated_speed = level * 0.01  # 线速度估算：每档位约0.01m/s
                complete_table[level] = {
                    'speed_ms' if self.calibration_type == "linear" else 'angular_speed_deg_s': estimated_speed,
                    'source': 'estimated'
                }
        
        return complete_table
    
    def save_calibration_results(self, filename: str = None) -> str:
        """
        保存标定结果
        Save calibration results
        
        Args:
            filename: 文件名 Filename
            
        Returns:
            str: 保存的文件路径 Saved file path
        """
        if filename is None:
            timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
            calibration_prefix = "linear" if self.calibration_type == "linear" else "angular"
            filename = f"{calibration_prefix}_speed_calibration_30levels_{timestamp}.json"
        
        # 生成完整标定表
        complete_table = self.generate_complete_calibration_table()
        
        # 准备保存数据
        metadata = {
            'calibration_type': self.calibration_type,
            'timestamp': datetime.now().isoformat(),
            'calibration_levels': self.calibration_levels,
            'calibration_repeats': self.calibration_repeats,
            'total_levels': 30
        }
        
        # 只有线速度标定才需要calibration_distance_m
        if self.calibration_type == "linear" and hasattr(self, 'calibration_distance_m'):
            metadata['calibration_distance_m'] = self.calibration_distance_m
        
        save_data = {
            'metadata': metadata,
            'calibrated_data': self.calibration_data,
            'interpolated_data': self.interpolated_data,
            'complete_table': complete_table
        }
        
        # 保存到文件
        try:
            with open(filename, 'w', encoding='utf-8') as f:
                json.dump(save_data, f, indent=2, ensure_ascii=False)
            
            print(f"\n💾 标定结果已保存到: {filename}")
            return filename
            
        except Exception as e:
            print(f"❌ 保存失败: {e}")
            return None
    
    def load_calibration_results(self, filename: str) -> bool:
        """
        加载标定结果
        Load calibration results
        
        Args:
            filename: 文件名 Filename
            
        Returns:
            bool: 加载是否成功 Whether loading was successful
        """
        try:
            with open(filename, 'r', encoding='utf-8') as f:
                data = json.load(f)
            
            self.calibration_data = data.get('calibrated_data', {})
            self.interpolated_data = data.get('interpolated_data', {})
            
            # 转换键为整数
            self.calibration_data = {int(k): v for k, v in self.calibration_data.items()}
            self.interpolated_data = {int(k): v for k, v in self.interpolated_data.items()}
            
            print(f"✅ 标定结果已从 {filename} 加载")
            return True
            
        except Exception as e:
            print(f"❌ 加载失败: {e}")
            return False
    
    def print_calibration_summary(self):
        """
        打印标定结果摘要
        Print calibration results summary
        """
        print("\n📈 标定摘要")
        
        complete_table = self.generate_complete_calibration_table()
        
        # 根据标定类型确定速度键名和单位
        if self.calibration_type == "angular":
            speed_key = 'angular_speed_deg_s'
            speed_unit = '角速度(度/秒)'
        else:
            speed_key = 'speed_ms'
            speed_unit = '速度(m/s)'
        
        print(f"{'档位':<4} {speed_unit:<12} {'来源':<12}")
        print("-" * 36)
        
        for level in range(1, 31):
            if level in complete_table:
                speed = complete_table[level][speed_key]
                source = complete_table[level]['source']
                source_cn = {'calibrated': '标定', 'interpolated': '插值', 'estimated': '估算'}[source]
                print(f"{level:<4} {speed:<12.3f} {source_cn:<12}")
        
        print("\n📈 统计信息:")
        calibrated_count = len(self.calibration_data)
        interpolated_count = len(self.interpolated_data)
        estimated_count = 30 - calibrated_count - interpolated_count
        
        print(f"  标定档位: {calibrated_count}")
        print(f"  插值档位: {interpolated_count}")
        print(f"  估算档位: {estimated_count}")
        print(f"  总计档位: 30")
    
    def update_robot_config(self, config_path: str = "config/robot_config.json") -> bool:
        """
        将标定结果自动写入robot_config.json文件
        Automatically write calibration results to robot_config.json file
        
        Args:
            config_path: 配置文件路径 Config file path
            
        Returns:
            bool: 是否成功更新 Whether update was successful
        """
        try:
            # 生成完整标定表
            complete_table = self.generate_complete_calibration_table()
            
            # 读取现有配置文件
            if os.path.exists(config_path):
                with open(config_path, 'r', encoding='utf-8') as f:
                    config_data = json.load(f)
            else:
                print(f"⚠️  配置文件不存在: {config_path}")
                return False
            
            # 根据标定类型更新对应的配置节
            if self.calibration_type == "angular":
                config_key = "angular_speed_calibration_30_levels"
                speed_key = "angular_speed_deg_s"
                print("\n🔄 更新角速度标定数据到配置文件...")
            else:
                config_key = "linear_speed_calibration_30_levels"
                speed_key = "speed_ms"
                print("\n🔄 更新线速度标定数据到配置文件...")
            
            # 准备更新数据
            calibration_data = {}
            if self.calibration_type == "angular":
                calibration_data["comment"] = "角速度标定数据 (度/秒) Angular speed calibration data (deg/s)"
            
            for level in range(1, 31):
                if level in complete_table:
                    calibration_data[str(level)] = complete_table[level][speed_key]
            
            # 更新配置文件
            config_data[config_key] = calibration_data
            
            # 保存配置文件
            with open(config_path, 'w', encoding='utf-8') as f:
                json.dump(config_data, f, indent=2, ensure_ascii=False)
            
            print(f"✅ 配置文件已更新: {config_path}")
            print(f"📊 已更新 {len(calibration_data)-1 if 'comment' in calibration_data else len(calibration_data)} 个档位的数据")
            return True
            
        except Exception as e:
            print(f"❌ 更新配置文件失败: {e}")
            return False
    
    def load_and_update_config_from_file(self, calibration_file: str, config_path: str = "config/robot_config.json") -> bool:
        """
        从指定的标定文件载入数据并更新到robot_config.json
        Load calibration data from specified file and update robot_config.json
        
        Args:
            calibration_file: 标定文件路径 Calibration file path
            config_path: 配置文件路径 Config file path
            
        Returns:
            bool: 是否成功更新 Whether update was successful
        """
        try:
            # 检查标定文件是否存在
            if not os.path.exists(calibration_file):
                print(f"❌ 标定文件不存在: {calibration_file}")
                return False
            
            # 载入标定文件
            with open(calibration_file, 'r', encoding='utf-8') as f:
                calibration_data = json.load(f)
            
            # 检查标定文件格式 - 支持两种格式
            if 'calibration_data' not in calibration_data and 'complete_table' not in calibration_data:
                print(f"❌ 标定文件格式错误: 缺少calibration_data或complete_table字段")
                return False
            
            # 检查标定类型
            calibration_type = calibration_data.get('metadata', {}).get('calibration_type', 'unknown')
            if calibration_type not in ['linear', 'angular']:
                print(f"❌ 未知的标定类型: {calibration_type}")
                return False
            
            # 读取现有配置文件
            if os.path.exists(config_path):
                with open(config_path, 'r', encoding='utf-8') as f:
                    config_data = json.load(f)
            else:
                print(f"❌ 配置文件不存在: {config_path}")
                return False
            
            # 根据标定类型确定配置键名
            if calibration_type == "angular":
                config_key = "angular_speed_calibration_30_levels"
                speed_key = "angular_speed_deg_s"
                print(f"\n🔄 从文件载入角速度标定数据: {calibration_file}")
            else:
                config_key = "linear_speed_calibration_30_levels"
                speed_key = "speed_ms"
                print(f"\n🔄 从文件载入线速度标定数据: {calibration_file}")
            
            # 准备更新数据
            new_calibration_data = {}
            if calibration_type == "angular":
                new_calibration_data["comment"] = "角速度标定数据 (度/秒) Angular speed calibration data (deg/s)"
            
            # 转换标定数据格式 - 支持两种数据格式
            if 'complete_table' in calibration_data:
                # 新格式：使用complete_table
                for level_str, level_data in calibration_data['complete_table'].items():
                    if speed_key in level_data:
                        new_calibration_data[level_str] = level_data[speed_key]
            elif 'calibration_data' in calibration_data:
                # 旧格式：使用calibration_data
                for level_str, level_data in calibration_data['calibration_data'].items():
                    if speed_key in level_data:
                        new_calibration_data[level_str] = level_data[speed_key]
            
            # 更新配置文件
            config_data[config_key] = new_calibration_data
            
            # 保存配置文件
            with open(config_path, 'w', encoding='utf-8') as f:
                json.dump(config_data, f, indent=2, ensure_ascii=False)
            
            print(f"✅ 配置文件已更新: {config_path}")
            print(f"📊 已载入 {len(new_calibration_data)-1 if 'comment' in new_calibration_data else len(new_calibration_data)} 个档位的{('角速度' if calibration_type == 'angular' else '线速度')}数据")
            return True
            
        except Exception as e:
            print(f"❌ 载入标定文件失败: {e}")
            return False
    
    def run_full_calibration(self) -> str:
        """
        运行完整的交互式标定流程
        Run complete interactive calibration process
        
        Returns:
            str: 保存的文件路径 Saved file path
        """
        print("\n🎯 30档速度交互式标定系统")
        print("=" * 50)
        print("\n欢迎使用六足机器人速度标定系统！")
        print("\n本系统将帮助您标定机器人的30个速度档位。")
        print("标定过程中，机器人会以不同档位移动3秒，")
        print("您需要测量实际移动距离并输入系统。")
        print("\n🚀 标定流程开始...")
        
        try:
            # 1. 标定固定间隔档位
            print("\n📊 第一阶段：固定间隔档位标定")
            print("我们将标定以下档位:", self.calibration_levels)
            calibration_results = self.calibrate_fixed_levels()
            
            if not calibration_results:
                print("❌ 固定间隔标定失败，无法继续")
                return None
            
            # 询问是否继续插值
            print("\n" + "="*50)
            print("第一阶段完成！")
            continue_interpolation = input("是否继续进行插值计算其他档位？(y/n): ")
            
            if continue_interpolation.lower() == 'y':
                # 2. 插值计算其他档位
                print("\n🔄 第二阶段：插值计算其他档位")
                self.interpolate_missing_levels()
            else:
                print("跳过插值计算，仅保存已标定的档位数据")
            
            # 3. 打印摘要
            print("\n📈 标定摘要")
            self.print_calibration_summary()
            
            # 4. 保存结果
            print("\n💾 保存标定结果")
            save_results = input("是否保存标定结果到文件？(y/n): ")
            filename = None
            if save_results.lower() == 'y':
                filename = self.save_calibration_results()
                if filename:
                    print(f"✅ 标定数据已保存到: {filename}")
            
            print("\n🎉 标定流程完成！")
            print("感谢您的耐心配合！")
            return filename
            
        except KeyboardInterrupt:
            print("\n\n⚠️  用户中断了标定流程")
            save_partial = input("是否保存已完成的标定数据？(y/n): ")
            if save_partial.lower() == 'y':
                filename = self.save_calibration_results()
                if filename:
                    print("✅ 部分标定数据已保存")
                return filename
            return None
        except Exception as e:
            print(f"❌ 标定过程出错: {e}")
            import traceback
            traceback.print_exc()
            return None

def main():
    """
    主函数
    Main function
    """
    print("🤖 六足机器人30档速度交互式标定系统")
    print("=" * 50)
    print("\n系统功能:")
    print("1. 交互式速度标定 - 逐档位进行精确标定")
    print("2. 支持线速度和角速度独立标定")
    print("3. 实时用户输入 - 根据实际测量输入速度值")
    print("4. 智能插值计算 - 自动计算未标定档位的速度")
    print("5. 数据保存加载 - 支持标定结果的持久化存储")
    
    print("\n⚠️  使用前请确保:")
    print("- 机器人已正确连接并初始化")
    print("- 有足够的安全移动空间（建议3米以上）")
    print("- 准备好测量工具（卷尺、量角器等）")
    print("- 确保机器人电量充足")
    
    # 选择功能模式
    print("\n📋 请选择功能模式:")
    print("1. 线速度标定 (m/s) - 测试前进后退左右平移速度")
    print("2. 角速度标定 (deg/s) - 测试旋转速度")
    print("3. 载入角速度标定文件到配置 - 从现有文件载入角速度数据")
    print("4. 载入线速度标定文件到配置 - 从现有文件载入线速度数据")
    
    while True:
        mode_choice = input("\n请选择功能模式 [1/2/3/4]: ").strip()
        if mode_choice == '1':
            calibration_type = "linear"
            mode = "calibrate"
            print("✅ 已选择线速度标定模式")
            break
        elif mode_choice == '2':
            calibration_type = "angular"
            mode = "calibrate"
            print("✅ 已选择角速度标定模式")
            break
        elif mode_choice == '3':
            calibration_type = "angular"
            mode = "load_file"
            print("✅ 已选择载入角速度标定文件模式")
            break
        elif mode_choice == '4':
            calibration_type = "linear"
            mode = "load_file"
            print("✅ 已选择载入线速度标定文件模式")
            break
        else:
            print("❌ 无效选择，请输入 1、2、3 或 4")
    
    try:
        if mode == "calibrate":
            # 标定模式
            print("\n🚀 开始标定流程...")
            
            # 初始化标定器
            print("\n🔧 初始化标定系统...")
            calibrator = SpeedCalibration30Levels(debug=True, calibration_type=calibration_type)
            print(f"✅ 标定系统初始化完成 - {('线速度' if calibration_type == 'linear' else '角速度')}标定模式")
            
            # 运行完整标定
            result_file = calibrator.run_full_calibration()
            
            if result_file:
                print(f"\n🎊 恭喜！标定完成！")
                print(f"📁 结果已保存到: {result_file}")
                
                # 询问是否查看结果
                view_results = input("\n是否查看标定结果摘要？(y/n): ")
                if view_results.lower() == 'y':
                    print("\n📖 显示标定结果:")
                    print("✅ 使用刚完成的标定数据")
                    
                    # 直接使用当前标定器实例显示结果
                    calibrator.print_calibration_summary()
                
                # 询问是否自动写入配置文件
                if calibrator.calibration_type == "angular":
                    write_config = input("\n是否将角速度标定结果自动写入robot_config.json？(y/n): ")
                    if write_config.lower() == 'y':
                        if calibrator.update_robot_config():
                            print("🎉 角速度标定结果已成功写入配置文件！")
                        else:
                            print("❌ 写入配置文件失败")
            else:
                print("\n⚠️  标定未完成或被取消")
                print("您可以稍后重新运行程序继续标定")
                
        elif mode == "load_file":
            # 载入文件模式
            print(f"\n📂 载入{('角速度' if calibration_type == 'angular' else '线速度')}标定文件模式")
            
            # 列出当前目录下的标定文件
            import glob
            pattern = f"*{calibration_type}*calibration*.json"
            calibration_files = glob.glob(pattern)
            
            if not calibration_files:
                print(f"❌ 当前目录下未找到{('角速度' if calibration_type == 'angular' else '线速度')}标定文件")
                print(f"请确保文件名包含 '{calibration_type}' 和 'calibration' 关键词")
                return
            
            print(f"\n📋 找到以下{('角速度' if calibration_type == 'angular' else '线速度')}标定文件:")
            for i, file in enumerate(calibration_files, 1):
                print(f"{i}. {file}")
            
            # 让用户选择文件
            while True:
                try:
                    file_choice = input(f"\n请选择要载入的文件 [1-{len(calibration_files)}]: ").strip()
                    file_index = int(file_choice) - 1
                    if 0 <= file_index < len(calibration_files):
                        selected_file = calibration_files[file_index]
                        print(f"✅ 已选择文件: {selected_file}")
                        break
                    else:
                        print(f"❌ 无效选择，请输入 1 到 {len(calibration_files)} 之间的数字")
                except ValueError:
                    print("❌ 请输入有效的数字")
            
            # 创建标定器实例并载入文件
            calibrator = SpeedCalibration30Levels(debug=True, calibration_type=calibration_type)
            
            if calibrator.load_and_update_config_from_file(selected_file):
                print(f"🎉 {('角速度' if calibration_type == 'angular' else '线速度')}标定数据已成功载入到配置文件！")
            else:
                print("❌ 载入标定文件失败")
            
    except KeyboardInterrupt:
        print("\n\n👋 用户中断，程序退出")
    except Exception as e:
        print(f"\n❌ 程序运行出错: {e}")
        import traceback
        traceback.print_exc()
    
    print("\n👋 感谢使用六足机器人标定系统！")

if __name__ == "__main__":
    main()